// app
#include "robot_def.h"
#include "robot_cmd.h"
#include "shoot.h"
#include "step.h"

// module
#include "remote_control.h"
#include "ins_task.h"
#include "master_process.h"
#include "message_center.h"
#include "general_def.h"
#include "dji_motor.h"
#include "bmi088.h"
#include "rm_referee.h"
#include "referee_protocol.h"

// bsp
#include "bsp_dwt.h"
#include "bsp_log.h"
#include "bsp_pwm.h"
#include <stdbool.h>

static Publisher_t *shoot_cmd_pub;           // 发射控制消息发布者
static Subscriber_t *shoot_feed_sub;         // 发射反馈信息订阅者
static Shoot_Ctrl_Cmd_s shoot_cmd_send;      // 传递给发射的控制信息
static Shoot_Upload_Data_s shoot_fetch_data; // 从发射获取的反馈信息

static Publisher_t *dartstand_cmd_pub_x;   // 飞镖架控制消息发布者
static Subscriber_t *dartstand_feed_sub_x; // 飞镖架馈信息订阅者
static Stand_Ctrl_Cmd_s_x stand_cmd_send_x;      // 发送给飞镖架应用的信息
static Stand_Upload_Data_s stand_fetch_data_x; // 从飞镖架应用接收的反馈信息信息

static Publisher_t *dartstand_cmd_pub_y;   // 飞镖架控制消息发布者
static Subscriber_t *dartstand_feed_sub_y; // 飞镖架馈信息订阅者
static Stand_Ctrl_Cmd_s_y stand_cmd_send_y;      // 发送给飞镖架应用的信息
static Stand_Upload_Data_s stand_fetch_data_y; // 从飞镖架应用接收的反馈信息信息

static Robot_Status_e robot_state; // 机器人整体工作状态
static RC_ctrl_t *rc_data;         // 遥控器数据

static PWMInstance *step_pwm1; //电推杆pwm控制实例
static PWMInstance *step_pwm2; //电推杆pwm控制实例

//电推杆控制距离的定时参数
static uint8_t tool = 0;
static double result = 0;
static uint8_t period = 0;
static uint8_t shoot_state = 0;

//逻辑判断位
static uint8_t flag = 0;
static uint8_t last_flag = 0;

//击打目标切换逻辑位
static uint8_t target_states = 0;
#define  outpost_target 0
#define base_target 1

//可供改变的速度，相关代码已被注释
double w_speed =0.0;

//逻辑判断位
static uint8_t last_key;

//裁判系统相关数据
static uint16_t referee_change=0; // 飞镖打击目标切换指令时间初始值，裁判系统可以读到切换的时间，初始为0
static uint16_t referee_launch=0; // 飞镖发射指令时间初始值，其他同上
static referee_info_t *referee_info; //裁判系统数据初始化
static uint8_t launch_state = 0;
static uint8_t change_state = 0;

//发射速度
static int outpost_speed = 27450 ;
static int base_speed = 34200-250;

#define Yes 1
#define No 0

//裁判系统判断是否发射
uint8_t dart_referee_launch()
{
    // 飞镖这门开启先
    if(referee_info->DartClientCmd.dart_launch_opening_status == 0)
    {
        //如果和保存的最后一次云台手发射的指令不一样，即判断为要发射
        if(referee_info->DartClientCmd.latest_launch_cmd_time != referee_launch)
        {
            referee_launch = referee_info->DartClientCmd.latest_launch_cmd_time;
            return Yes;
        }
        return No;
    }
    return No;
}

// 裁判系统判断是否切换击打目标
uint8_t dart_referee_change()
{   
    //这个貌似不需要飞镖闸门开启？和保存的最后一次云台手发射的指令不一样，即判断为要切换（虽然貌似一局只会有一次）
    if(referee_info->DartClientCmd.target_change_time != referee_change )
    {
        referee_change = referee_info->DartClientCmd.target_change_time;
        return Yes;
    }
    return No;
}

//pwm及消息中心及基础初始化
void RobotCMDInit()
{
    referee_info = RefereeInit(&huart6);
    rc_data = RemoteControlInit(&huart3);   // 修改为对应串口,注意如果是自研板dbus协议串口需选用添加了反相器的那个

        PWM_Init_Config_s pwm_conf1 = {
        .dutyratio = 50,
        .htim = &htim1,
        .period = 1,
        .channel = TIM_CHANNEL_1,
    };
    step_pwm1 = PWMRegister(&pwm_conf1);
    PWM_Init_Config_s pwm_conf2 = {
        .dutyratio = 50,
        .period = 1,
        .channel = TIM_CHANNEL_2,
        .htim = &htim1,
    };
    step_pwm2 = PWMRegister(&pwm_conf2);

    shoot_cmd_pub = PubRegister("shoot_cmd", sizeof(Shoot_Ctrl_Cmd_s));
    shoot_feed_sub = SubRegister("shoot_feed", sizeof(Shoot_Upload_Data_s));
    dartstand_cmd_pub_x = PubRegister("stand_cmd_x", sizeof(Stand_Ctrl_Cmd_s_x));
    dartstand_feed_sub_x = SubRegister("stand_feed_x", sizeof(Stand_Upload_Data_s));
    dartstand_cmd_pub_y = PubRegister("stand_cmd_y", sizeof(Stand_Ctrl_Cmd_s_y));
    dartstand_feed_sub_y = SubRegister("stand_feed_y", sizeof(Stand_Upload_Data_s));
    robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
}

//切换击打目标为前哨站,遥控器
void Step_outpost_task(){
    if(rc_data[TEMP].rc.rocker_r_ == -660 && target_states != outpost_target){
        Step_outpost();
        target_states = outpost_target;
    }
}

//切换击打目标为基地 ，遥控器
void Step_base_task(){
    if(rc_data[TEMP].rc.rocker_r_ == 660  && target_states != base_target){
        Step_base();
        target_states = base_target;
    }
}

//切换击打目标为前哨站 ，裁判系统
void Step_outpost_task_referee(){
    if(change_state == 1 && target_states != outpost_target){
        Step_outpost();
        target_states = outpost_target;
    }
}

//切换击打目标为基地 ，裁判系统
void Step_base_task_operator(){
    if(change_state == 1 && target_states != base_target){
        Step_base();
        target_states = base_target;
    }
}

//设定发射速度及发射
void speed_set_launch(){
    if (target_states == outpost_target){
        shoot_mini(outpost_speed);
    }
    else if (target_states == base_target){
        shoot_mini(base_speed);
    }
}

// 一键发射一发,遥控器
void one_pitch(){
        // shoot_cmd_send.shoot_mode = SHOOT_ON;
        // robot_state = ROBOT_READY;
        if(rc_data[TEMP].rc.rocker_r1 == 660){
        shoot_easy_mode();
        shoot_mini(base_speed);
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
        DWT_Delay(1.9);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
        DWT_Delay(2.2);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 0);
        angle();   
    }
}

// 一键发射两发，遥控器
void two_pitch(){
        // shoot_cmd_send.shoot_mode = SHOOT_ON;
        // robot_state = ROBOT_READY;
        if(rc_data[TEMP].rc.rocker_r1 == -660){
        shoot_easy_mode();
        speed_set_launch();
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
        DWT_Delay(1.9);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
        DWT_Delay(2.2);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 0);
        angle();
        DWT_Delay(0.3);
        shoot_easy_mode();
        speed_set_launch();
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
        DWT_Delay(1.9);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
        DWT_Delay(2.2);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 0);
        angle();
    }
}

// 裁判系统发射，两发
void two_pitch_referee(){
    if(launch_state == 1){
        shoot_cmd_send.shoot_mode = SHOOT_ON;
        robot_state = ROBOT_READY;
        shoot_easy_mode();
        shoot_mini(base_speed);
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
        DWT_Delay(1.9);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
        DWT_Delay(2.2);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 0);
        angle();
        DWT_Delay(0.3);
        shoot_easy_mode();
        shoot_mini(base_speed);
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
        DWT_Delay(1.9);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
        DWT_Delay(2.2);
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 0);
        angle();
    }
}

//裁判系统数据获取任务
void referee_task_data(){
    // change_state = dart_referee_change();
    launch_state = dart_referee_launch();
}

//裁判系统任务
void referee_task(){
    //Step_outpost_task_referee();
    //Step_base_task_operator();
    two_pitch_referee();
}

//遥控器 
static void RemoteControlSet(){

    PWMSetDutyRatio(step_pwm1, 0);
    PWMSetDutyRatio(step_pwm2, 0);

    shoot_cmd_send.load_mode = LOAD_STOP;
    shoot_cmd_send.friction_mode = FRICTION_OFF;
    shoot_cmd_send.bullet_speed = Test_Speed;
    shoot_cmd_send.shoot_rate = 0.0f;

    //Step_outpost_task();
    //Step_base_task();

    //one_pitch();
    //two_pitch();

    if(rc_data->rc.dial < -300)
    {
        shoot_cmd_send.friction_mode = FRICTION_ON;
        shoot_cmd_send.shoot_mode = SHOOT_ON;
        robot_state = ROBOT_READY;
        // uart_send();
    }

    if( (rc_data->rc.switch_left == 1 && last_key == 3)  )
    {
        angle();
    }

    if(rc_data->rc.switch_left ==2 )
    {
        shoot_easy_mode();
    }

    else if(rc_data->rc.switch_right == 1)
    {
        PWMSetDutyRatio(step_pwm1, 100);
        PWMSetDutyRatio(step_pwm2, 0);
    }   

    else if(rc_data->rc.switch_right == 2)
    {
        PWMSetDutyRatio(step_pwm1, 0);
        PWMSetDutyRatio(step_pwm2, 100);
    }   

    last_key = rc_data->rc.switch_left;

    stand_cmd_send_x.dx = 1000.0f * ((float)rc_data[TEMP].rc.rocker_l_)/660.0f; // 右水平方向
    stand_cmd_send_y.dy = 1000.0f * ((float)rc_data[TEMP].rc.rocker_l1)/660.0f; // 左竖直方向
    stand_cmd_send_x.v = 7*720.0f * 0.1f; // 移动速度
    stand_cmd_send_y.v = 180.0f * 0.1f; // 移动速度

    w_speed = ((float)rc_data[TEMP].rc.rocker_r1/660.0f)*25000;
    
}

/**
 * @brief  紧急停止,包括遥控器左上侧拨轮打满/重要模块离线/双板通信失效等
 *         停止的阈值'300'待修改成合适的值,或改为开关控制.
 *
 * @todo   后续修改为遥控器离线则电机停止(关闭遥控器急停),通过给遥控器模块添加daemon实现
 *
 */

static void EmergencyHandler()
{
    // 拨轮的向下拨超过一半进入急停模式.注意向打时下拨轮是正
    if (rc_data[TEMP].rc.dial > 300 || robot_state == ROBOT_STOP) // 还需添加重要应用和模块离线的判断
    {
        shoot_cmd_send.shoot_mode = SHOOT_OFF;
        shoot_cmd_send.friction_mode = FRICTION_OFF;
        shoot_cmd_send.load_mode = LOAD_STOP;
        robot_state = ROBOT_STOP;
    }
}

//     // 遥控器右侧开关为[上],恢复正常运行
//     if (rc_data->rc.switch_right == 1)
//     {
//         shoot_cmd_send.shoot_mode = SHOOT_ON;
//         robot_state = ROBOT_READY;
//         //uart_receive();
//     }
// }

/* 机器人核心控制任务,200Hz频率运行 */
void RobotCMDTask()
{
    SubGetMessage(shoot_feed_sub, &shoot_fetch_data);
    SubGetMessage(dartstand_feed_sub_x, &stand_fetch_data_x);
    SubGetMessage(dartstand_feed_sub_y, &stand_fetch_data_y);   
    // CalcOffsetAngle();

    RemoteControlSet();
    EmergencyHandler();

    referee_task_data();
    referee_task();

    PubPushMessage(shoot_cmd_pub, (void *)&shoot_cmd_send);
    PubPushMessage(dartstand_cmd_pub_x, (void *)&stand_cmd_send_x);
    PubPushMessage(dartstand_cmd_pub_y, (void *)&stand_cmd_send_y);  
}
